#!/usr/bin/env roseus

#-:jsk
(jsk)
#-:rbrain-basic
(rbrain)

(load "package://mrobot_ros_bridge/model/beegomodel.l")

(ros::roseus "beego_robot_publisher")

(setq *tfb* (instance ros::transform-broadcaster :init))
(setq *tfl* (instance ros::transform-listener :init))
(setq *robot-msgs* nil)

;; (if (ros::has-param "~robot")
;;     (setq *robot-forms* (read-from-string (ros::get-param "~robot"))))

(let* ((robot (beego-new))
       (robot-name "beego")
       (robot-frame-id "/beego"))
  (ros::advertise robot-frame-id visualization_msgs::Marker 5)
  (ros::ROS-INFO "loading and advertise ~a" robot-name)
  (setq *robot-msg* (object->marker-msg robot
                                (instance std_msgs::header
                                          :init :stamp (ros::time-now)
                                          :frame_id robot-frame-id)))
  (ros::ROS-INFO "~a ~a ~a" robot-name (send robot :worldpos)
                 (ros::tf-point->pos (send *robot-msg* :pose :position)))
  (setq *base_link->marker-trans* (make-coords)))

(ros::rate 5)
(do-until-key
 (let (frame-id cds)
   (setq frame-id (send *robot-msg* :header :frame_id))
   (print (list *robot-msg* frame-id))
   (send *robot-msg* :header :stamp (ros::time-now))
   (ros::ROS-INFO "publish marker ~A" frame-id)
   (ros::publish frame-id *robot-msg*)
   (send *tfb* :send-transform *base_link->marker-trans* "/base_link" frame-id)
   )
 (ros::spin-once)
 (ros::sleep))